﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class WorldGrid : MonoBehaviour {

    public static WorldGrid Instance
    {
        get;
        private set;
    }

    public Transform test;

    const int rs = 64;
    const int hrs = 32;
    
    public float gridSize = 0.5f;
    public int regionCount;
    Dictionary<int, Dictionary<int, Region>> regions = new Dictionary<int, Dictionary<int, Region>>();

    public LayerMask worldMask;

    
    Dictionary<int, Color> colors = new Dictionary<int, Color>();

    void Awake()
    {
        Instance = this;
        colors.Add(1 << LayerMask.NameToLayer("World"), Color.white);
        colors.Add(1 << LayerMask.NameToLayer("Tree"), Color.green);
        colors.Add(1 << LayerMask.NameToLayer("Water"), Color.blue);
        colors.Add(1 << LayerMask.NameToLayer("Blocked"), Color.black);
    }

    public Field this[Vector3 pos]
    {
        get
        {
            return this[Mathf.RoundToInt(pos.x / gridSize - gridSize), Mathf.RoundToInt(pos.y / gridSize - gridSize)];
        }
    }

    public Field this[int x, int y]
    {
        get
        {
            int rx = Mathf.RoundToInt((x + 0.5f) / rs);
            int ry = Mathf.RoundToInt((y + 0.5f) / rs);
            //int rx = ((x + hrs) % rs < 0) ? (x + hrs) / rs + 1 : (x + hrs) / rs;
            //int ry = (y + hrs) / rs - (y < 0 ? 1 : 0);
            //print("r " + rx + " " + ry);
            if (!regions.ContainsKey(rx))
                regions.Add(rx, new Dictionary<int, Region>());
            if (!regions[rx].ContainsKey(ry))
            {
                regionCount++;
                regions[rx].Add(ry, new Region(rx, ry, gridSize));
            }
            return regions[rx][ry][x, y];
        }
    }



    public class Region
    {
        public Field this[int x, int y]
        {
            get
            {
                //x += hrs - ;
                //y += hrs;
                //int gx = x % rs;
                //if (gx < 0)
                //    gx += rs;
                //int gy = y % rs;
                //if (gy < 0)
                //    gy += rs;
                //print("f " + (x - mx) + " " + (y - my) + " w " + x + " " + y);
                var f = grid[x - mx, y - my];
                //print("fc " + x + "=" + f.x + " " + y + "=" + f.y);
                return f;
            }
        }
        Field[,] grid = new Field[rs,rs];
        public Bounds bounds;
        int mx, my;

        public Region(int x, int y, float gridSize)
        {
            mx = x * rs - hrs;
            my = y * rs - hrs;
            //print(x + " " + y + " > " + mx + " " + my);
            bounds = new Bounds(new Vector3(x * rs * gridSize, y * rs * gridSize, 0), new Vector3(rs * gridSize, rs * gridSize, 1));
            print(bounds);
            Vector3 min = bounds.min + new Vector3(gridSize / 2, gridSize / 2, 0.5f);

            for (int vx = 0; vx < rs; vx++)
            {
                for (int vy = 0; vy < rs; vy++)
                {
                    var f = new Field(x * rs + vx - hrs, y * rs + vy - hrs, min + new Vector3(vx * gridSize, vy * gridSize));
                    grid[vx, vy] = f;
                }
            }
        }
    }

    public class Field
    {
        public Vector3 position;
        public int cost;
        public int h;
        public int itteration;
        public int visited;
        public int mask;
        public Field last;
        public int q;

        public int x, y;
        Dictionary<Field, int> neighbors = null;
        public Dictionary<Field, int> Neighbors
        {
            get
            {
                if (neighbors == null)
                {
                    neighbors = new Dictionary<Field, int>(8);
                    neighbors.Add(WorldGrid.Instance[x - 1, y], 10);
                    neighbors.Add(WorldGrid.Instance[x + 1, y], 10);
                    neighbors.Add(WorldGrid.Instance[x, y - 1], 10);
                    neighbors.Add(WorldGrid.Instance[x, y + 1], 10);
                    neighbors.Add(WorldGrid.Instance[x - 1, y - 1], 15);
                    neighbors.Add(WorldGrid.Instance[x - 1, y + 1], 15);
                    neighbors.Add(WorldGrid.Instance[x + 1, y - 1], 15);
                    neighbors.Add(WorldGrid.Instance[x + 1, y + 1], 15);
                }
                return neighbors;
            }
        }

        public Field(int x, int y, Vector3 pos)
        {
            this.x = x;
            this.y = y;
            position = pos;
            RaycastHit info;
            if (Physics.Raycast(pos + Vector3.forward * 5, Vector3.back, out info, 10, WorldGrid.Instance.worldMask))
            {
                mask = 1 << info.transform.gameObject.layer;
            }
            else
            {
                mask = 1 << LayerMask.NameToLayer("World");
            }

        }

        public override string ToString()
        {
            return "Field: " + x + " * " + y + " m: " + mask;
        }
    }

    // Update is called once per frame
    void Update()
    {
        if (Input.GetMouseButtonDown(0))
            print(this[Camera.main.ScreenToWorldPoint(Input.mousePosition)]);
	}

    Field current = null;
    void OnDrawGizmosSelected()
    {
        foreach (var rx in regions)
        {
            foreach (var ry in rx.Value)
            {
                for (int x = -hrs; x < hrs; x++)
                {
                    for (int y = -hrs; y < hrs; y++)
                    {
                        var f = ry.Value[rx.Key * rs + x, ry.Key * rs +  y];
                        Gizmos.color = colors[f.mask];
                        Gizmos.DrawCube(f.position, Vector3.one * (gridSize / 3));
                    }
                }
            }
        }

        if (current != null)
        {
            Gizmos.color = Color.red;
            Gizmos.DrawCube(current.position, Vector3.one);
            int safe = 100;
            for (Field l = current.last; l != null; l = l.last)
            {
                Gizmos.DrawCube(l.position, Vector3.one * 0.5f);
                if (safe-- < 0)
                    break;
            }
        }
    }

    int itteration;

    public List<Vector3> FindPath(Vector3 from, Vector3 to, int mask = 0)
    {
        var first = this[from];
        var last = this[to];

        if (!TestDestination(from, ref last, mask))
            return null;

        itteration++;
        q.Clear();

        first.itteration = itteration;
        first.last = null;
        first.cost = 0;
        q.Put(first);

        int safetyBreak = 100000;
        while (q.Any)
        {
            if (TestField(q.Top, last, mask))
                break;

            if (safetyBreak-- < 0 || q.AlmostFull)
            {
                Debug.LogError("tires left: " + safetyBreak + " Queue full: " + q.AlmostFull);
                return null;
            }
        }

        var output = new List<Vector3>();
        for (Field l = last; l != null; l = l.last)
        {
            if (l == first)
                break;
            output.Add(l.position);
        }
        return output;
    }

    bool TestDestination(Vector3 from, ref Field to, int mask)
    {
        if ((to.mask & mask) == to.mask)
            return true;
        var dir = (from - to.position).normalized * gridSize;
        var alt = this[to.position + dir];
        if ((alt.mask & mask) == alt.mask)
        {
            to = alt;
            return true;
        }
        foreach (var a in to.Neighbors)
        {
            if ((a.Key.mask & mask) == a.Key.mask)
            {
                to = a.Key;
                return true;
            }
        }
        return false;
    }

    bool TestField(Field field, Field to, int mask)
    {
        field.visited = itteration;
        if (field == to)
        {
            current = field;
            return true;
        }
        //print("Test: " + field);
        foreach (var n in field.Neighbors)
        {
            var f = n.Key;
            int c = field.cost + n.Value;
            int h = (Mathf.Abs(f.x - field.x) + Mathf.Abs(f.y - field.y)) * 10;
            if (f.itteration != itteration)
            {
                f.itteration = itteration;
                if ((f.mask & mask) == f.mask)
                {
                    f.cost = c;
                    f.h = h;
                    f.last = field;
                    q.Put(f);
                    //print("p x" + f.x + " y" + f.y + " c" + f.cost + " h" + f.h);
                }
                else
                {
                    f.cost = 0;
                    f.h = 0;
                    f.last = null;
                }
            }
            else if (f.visited != itteration &&  h + c < f.h + f.cost)
            {
                f.cost = c;
                f.h = h;
                f.last = field;
                q.Put(f);
            }
        }
        return false;
    }

    Queue q = new Queue();
    private class Queue
    {
        private Field[] queue = new Field[4096];
        private int bottom;

        private Field this[int i]
        {
            get
            {
                return queue[i];
            }
            set
            {
                queue[i] = value;
                value.q = i;
            }
        }

        public void Clear()
        {
            bottom = 0;
        }

        public bool AlmostFull
        {
            get
            {
                return queue.Length - bottom < 20;
            }
        }

        public bool Any
        {
            get
            {
                return bottom > 0;
            }
        }

        public Field Top
        {
            get
            {
                return PopPath();
            }
        }

        public void Put(Field f)
        {
            if (f.q == 0 || queue[f.q] != f)
                PutPath(f);
            else
                FloatPath(f);
        }

        private void PutPath(Field field)
        {
            //Debug.Log("put path: " + gridField.x + " " + gridField.y);
            bottom++;
            this[bottom] = field;
            FloatPath(field);
        }

        private void FloatPath(Field field)
        {
            int test = field.q;
            while (test > 1)
            {
                var parent = this[test / 2];
                if (parent.h + parent.cost > field.h + field.cost)
                {
                    this[test / 2] = field;
                    this[test] = parent;
                    test = test / 2;
                }
                else
                {
                    break;
                }
            }
        }

        private Field PopPath()
        {
            //Debug.Log("pop path");
            var top = this[1];
            var sink = this[bottom];
            this[1] = sink;
            bottom--;

            int test = 1;
            while (test * 2 <= bottom)
            {
                var a = this[test * 2];

                if (test * 2 + 1 <= bottom)
                {
                    var b = this[test * 2 + 1];
                    if (a.h + a.cost < b.h + b.cost)
                    {
                        if (a.h + a.cost < sink.h + sink.cost)
                        {
                            this[test] = a;
                            test = test * 2;
                            this[test] = sink;
                        }
                        else
                        {
                            break;
                        }
                    }
                    else
                    {
                        if (b.h + b.cost < sink.h + sink.cost)
                        {
                            this[test] = b;
                            test = test * 2 + 1;
                            this[test] = sink;
                        }
                        else
                        {
                            break;
                        }
                    }
                }
                else if (a.h + a.cost < sink.h + sink.cost)
                {
                    this[test] = a;
                    test = test * 2;
                    this[test] = sink;
                }
                else
                {
                    break;
                }
            }

            top.q = 0;
            return top;
        }
    }
}
